﻿#include "maindeal.h"

Maindeal::Maindeal( int argc, char** argv, QObject* parent ) : QObject( parent ), argc( argc ), argv( argv ) {}

Maindeal::~Maindeal() {
    udpThread->quit();
}

Agvhardware* Maindeal::agvhardware() const {
    return agvhardware_;
}

Navigator* Maindeal::navigator() const {
    return navigator_;
}

void Maindeal::sltTimeOut() {
    static int time1 = 0;
    if ( ++time1 >= ( Param::SysCommunicationFrequency / 10 ) ) {  //  4.6 调动通讯频率可调
        AgvToSystemOrder();
        time1 = 0;
    }

    //暂时弃用
    //  static int time2 = 0;
    //  if (++time2 >= (Param::SysPoseCommunicationFrequency / 10)) {
    //    AgvToSysPoseOrder();
    //    time2 = 0;
    //  }
}

void Maindeal::sltUdpRecvData( const QString& ip, const QByteArray& data ) {
    emit sigRecvSystemOrder( data, "Recv:[" + ip + "]" );  // 接收到的数据显示到debug中
    if ( data.size() == sys_recv_size ) {                  //调度系统数据8位
        sysRecvData( data );
    }
}

// Maindeal 接收调度数据处理
void Maindeal::sysRecvData( const QByteArray& data ) {
    if ( data.size() != sys_recv_size ) { return; }

    //数据校验，错误直接返回
    if ( ( static_cast< uchar >( data.at( 0 ) ) != 0xb1 ) || ( static_cast< uchar >( data.at( 1 ) ) != Param::agv_id ) ) { return; }

    sys_line = static_cast< uchar >( data.at( data.size() - 1 ) );  //显示线别

    if ( ( agvhardware_->getAgvState() == Agvhardware::AGV_EMERGENCY ) || !navigator()->isAutoModeEnable() ) {
        emit sigSendTips( "警告，AGV急停中或自动模式没开，无法执行调度指令" );
        is_order_error = true;
        return;
    }

    //收到调度数据后的处理
    uchar fun_code = static_cast< uchar >( data.at( 2 ) );

    switch ( fun_code ) {
    //路线
    case 1: {
        int Id = ( static_cast< uchar >( data.at( 4 ) ) << 8 ) + static_cast< uchar >( data.at( 3 ) );
        if ( navigator()->getSlamRoute() == Id ) {
            // UPDATE 2022.06.05
            // 重复路线判断！避免调度网络问题一直重复发生相同路线。
            emit sigSendTips( QString( "警告，无法执行调度指令,%1号路线已经设定，无需重复操作！" ).arg( Id ) );
        }
        else {                                                               // 判断线路
            is_order_error = !( navigator()->SystemNaviOrder( Id, true ) );  // 返回结果
        }
    } break;

    //升降

    case 2: {
        if ( agvhardware_->getAgvState() == Agvhardware::AGV_RUNNING ) {
            emit sigSendTips( "提示，AGV行驶中，无法执行调度下发的传输指令" );
            is_order_error = true;

            return;
        }
        uchar order = static_cast< uchar >( data.at( 5 ) );
        switch ( order ) {
        case 0x0:  //停止升降
            agvhardware_->setAgvState( Agvhardware::AGV_LIFT_STOP );
            agvhardware()->setForkliftState( Agvhardware::STOP );
            emit sigSendTips( "提示，调度要求强制执行传输暂停！" );
            break;
        case 0x1:  //上升
            if ( isAllowExecuteForklift() ) { agvhardware()->setForkliftState( Agvhardware::RAISE ); }
            else {
                is_order_error = true;
            }
            break;

        case 0x2:  //下降
            if ( isAllowExecuteForklift() ) { agvhardware()->setForkliftState( Agvhardware::FALL ); }
            else {
                is_order_error = true;
            }
            break;
        }
    } break;

    // 启停
    case 3: {
        switch ( static_cast< uchar >( data.at( 6 ) ) ) {
        case 1:  // 执行交管
            if ( agvhardware_->getAgvState() == Agvhardware::AGV_RUNNING ) {
                emit sigSendTips( "执行交通管制，停车" );
                navigator()->EmergencyStop();
                agvhardware_->setAgvState( Agvhardware::AGV_TRAFFIC );
            }
            break;
        case 2:  // 释放交管
            if ( agvhardware_->getAgvState() == Agvhardware::AGV_TRAFFIC ) {
                emit sigSendTips( "解除交通管制，重新开启导航" );
                agvhardware_->setAgvState( Agvhardware::AGV_RUNNING );
            }
            break;
        }
    } break;

    // UPDATE 2022.09.26 调度指令扩展
    case 4: {
        switch ( static_cast< uchar >( data.at( 6 ) ) ) {
        case 3:  // 取消任务-复位
            if ( is_need_cancel_mission ) { is_need_cancel_mission = false; }
            break;
        case 4:  // 触发急停
            navigator()->setEmergencyStop( true );
            break;
        case 5:  // 停止充电
            agvhardware()->stopCharge();
            break;
        }
    } break;

    //自动路线
    case 5:
    case 6:
    case 7: {
        int                          start_id   = ( static_cast< uchar >( data.at( 4 ) ) << 8 ) + static_cast< uchar >( data.at( 3 ) );
        int                          goal_id    = ( static_cast< uchar >( data.at( 6 ) ) << 8 ) + static_cast< uchar >( data.at( 5 ) );
        SlamRoutePlanning::RouteType route_type = fun_code == 6 ? SlamRoutePlanning::ENTER : fun_code == 7 ? SlamRoutePlanning::EXIT : SlamRoutePlanning::NORMAL;
        if ( navigator()->getSlamGoalId() == goal_id ) {
            // UPDATE 2022.06.05
            // 重复路线判断！避免调度网络问题一直重复发生相同路线。
            emit sigSendTips( QString( "警告，无法执行调度指令,路线(->%1)已经设定，无需重复操作！" ).arg( goal_id ) );
        }
        else {                                                                                           // 判断线路
            is_order_error = !( navigator()->SystemNaviOrder3( start_id, goal_id, route_type, true ) );  // 返回结果
        }
    } break;
    }
}

// void Maindeal::chargeRecvData(const QByteArray& data)  //  Maindeal 接收充电站数据处理
//{
//  // DELETE 接收充电站数据处理（中来5G 充电桩无通讯）
//  if (data.size() != 6) {
//    return;
//  }

//  int id = navigator()->getSlamId();  //
//  if (navigator()->coordianteRoutePlanning()->isMachineArriveNum(id)) {
//    return;
//  }

//  if ((static_cast<uchar>(data.at(0)) != 0xA2) || (static_cast<uchar>(data.at(1)) != static_cast<uchar>(Param::agv_id) ||
//                                                   (static_cast<uchar>((data.at(data.size() - 1))) != Utility::CKS(data, data.size() - 1)))) {
//    return;  //数据错误，直接返回
//  }

//  int charge_ip_num = navigator()->slamRoutePlanning()->getChargeIp(navigator()->getSlamId());

//  int charge_station_id = (static_cast<uchar>(data.at(3)) << 8) + static_cast<uchar>(data.at(2));
//  if (charge_station_id != charge_ip_num) {
//    return;  //数据错误，直接返回
//  }
//  emit sigSendChargeOrder(data, "Recv:");  // 接收到的数据显示到debug中，充电桩为广播发送，需要确保对应数据正确后再显示

//  if ((static_cast<uchar>(data.at(4) == 1)) && (agvhardware_->getAgvState() == Agvhardware::AGV_ARRIVE)) {
//    agvhardware_->setAgvState(Agvhardware::AGV_CHARGEING);  // 切换充电状态
//    emit sigSendTips("正确，AGV充电中");
//  }

//  if ((static_cast<uchar>(data.at(4) == 2)) && (agvhardware_->getAgvState() == Agvhardware::AGV_CHARGEING)) {
//    agvhardware_->setAgvState(Agvhardware::AGV_ARRIVE);
//    emit sigSendTips("正确，AGV解除充电");
//  }
//}

void Maindeal::AgvToSystemOrder() {  //  Maindeal 发送数据给调度
    QByteArray data( 17, 0x00 );
    int        route = navigator()->getSlamRoute();
    short int  x_cm  = static_cast< short int >( navigator()->slamPoint().x() / 10 );
    short int  y_cm  = static_cast< short int >( navigator()->slamPoint().y() / 10 );
    short int  rad   = static_cast< short int >( navigator()->slamPoint().radian() * 100 );
    int        i     = 0;
    data[ i++ ]      = static_cast< char >( 0xB1 );                                                                 // 包头
    data[ i++ ]      = static_cast< char >( Param::agv_id );                                                        // 车号
    data[ i++ ]      = static_cast< char >( agvhardware_->getAgvState() );                                          // AGV状态
    data[ i++ ]      = static_cast< char >( route );                                                                // 路线号低8
    data[ i++ ]      = static_cast< char >( route >> 8 );                                                           //
    data[ i++ ]      = static_cast< char >( navigator_->isForward() );                                              // 方向
    data[ i++ ]      = static_cast< char >( x_cm );                                                                 //
    data[ i++ ]      = static_cast< char >( x_cm >> 8 );                                                            //
    data[ i++ ]      = static_cast< char >( y_cm );                                                                 //
    data[ i++ ]      = static_cast< char >( y_cm >> 8 );                                                            //
    data[ i++ ]      = static_cast< char >( rad );                                                                  //弧度低8位  UPDATE 2022.06.01增加弧度发送给调度
    data[ i++ ]      = static_cast< char >( rad >> 8 );                                                             //弧度高8位
    data[ i++ ]      = static_cast< char >( is_order_error );                                                       // 收到的命令是否错误执行
    data[ i++ ]      = static_cast< char >( agvhardware_->Battery_Ropeencoder_canModule()->getBatteryData().soc );  // AGV电池容量百分比
    data[ i++ ]      = static_cast< char >( agvhardware_->getGoodsState() );                                        // 物料状态
    data[ i++ ]      = static_cast< char >( agvhardware_->getForkPosState() );                                      // Fork状态
    data[ i++ ]      = static_cast< char >( is_need_cancel_mission );                                               // 取消任务
    //  data[data.size() - 1] = static_cast<char>(Utility::CKS(data, data.size() - 1));                 // Check Sum

    if ( Param::sys_ip_and_port.size() > 0 ) {
        foreach ( Param::IpAndPort demo, Param::sys_ip_and_port ) {
            udp->sendData( demo.ip, demo.port, data );                   // UDP发送数据
            emit sigSendSystemOrder( data, "Send: [" + demo.ip + "]" );  // 将发送的数据显示出来监控
        }
    }
    is_order_error = false;  // 复位状态
}

void Maindeal::setCancelMission( bool sw ) {
    is_need_cancel_mission = sw;
}

bool Maindeal::isAllowExecuteForklift() {
    //  if (!agvhardware_->isAgvArriveSignal()) {
    //    emit sigSendTips("警告，到位光电传感器没触发，无法执行传输指令");
    //    return false;
    //  }

    //    if ( agvhardware_->isTranSmitEnable() ) {
    //        emit sigSendTips( "警告，传送带运行中，无法执行传输指令" );
    //        return false;
    //    }

    //  bool is_docking_machine = navigator()->isModeDucking();
    //  if (!is_docking_machine) {
    //    emit sigSendTips("警告，该模式不是对接机台，无法执行传输指令");
    //    return false;
    //  }

    if ( !agvhardware_->isAllowForklift() ) {
        emit sigSendTips( "警告，该状态禁止升降，无法执行传输指令" );
        return false;
    }

    //  int id = navigator()->getSlamId();
    //  if (!navigator()->coordianteRoutePlanning()->isMachineArriveNum(id)) {
    //    emit sigSendTips("警告，当前id不是机台id，无法执行传输指令");
    //    return false;
    //  }

    //  if (!navigator()->isNearTargetStation()) {
    //    emit sigSendTips("警告，与目标对接口坐标距离超过50mm，无法执行传输指令");
    //    return false;
    //  }
    return true;
}

// bool Maindeal::isAllowExitMachine() {
//  //  int id = navigator()->getSlamId();
//  if (agvhardware_->getAgvState() == Agvhardware::AGV_RUNNING) {
//    emit sigSendTips("提示，小车行驶中，无法执行退出指令");
//    return false;
//  }

//  //  if (navigator_->getNavigationMode() == Navigator::NAVIGATION_COOR) {
//  //    emit sigSendTips("提示，小车坐标导航中，无法执行退出指令");
//  //    return false;
//  //  }

//  //    if ( agvhardware_->isTransmitError() &&
//  //    navigator()->coordianteRoutePlanning()->isMachineArriveNum( id ) )
//  //    //机台退出，传输带前端有物体
//  //    {
//  //        emit sigSendTips( "警告，传送带最前端有物体，无法执行退出指令" );
//  //        agvhardware_->setCannotTransmit( true );
//  //        return false;
//  //    }
//  //    if ( agvhardware_->isTranSmitEnable() ) {
//  //        emit sigSendTips( "警告，传输任务进行中，无法执行退出指令" );
//  //        return false;
//  //    }

//  return true;
//}

int Maindeal::getSysLine() const {
    return sys_line;
}

/**
 * @brief MainDeal::sltMaindealInit 初始化 构建navigator对象和agvhardware对象。
 */
void Maindeal::sltMaindealInit() {  //  Maindeal 初始化数据
    // qDebug() << "miandeal_currentThreadId()==" << QThread::currentThreadId();

    udpThread = new QThread;
    udp       = new UDPUtils( Param::agv_ip, Param::agv_sys_data_port, false, this );
    udp->moveToThread( udpThread );
    connect( udpThread, SIGNAL( started() ), udp, SLOT( sltOpenUDP() ) );
    connect( udpThread, SIGNAL( finished() ), udpThread, SLOT( deleteLater() ) );
    udpThread->start();
    udpThread->setPriority( QThread::TimeCriticalPriority );
    // udp信号槽联系
    connect( udp, SIGNAL( sigReceiveData( QString, QByteArray ) ), this, SLOT( sltUdpRecvData( QString, QByteArray ) ) );
    connect( udp, SIGNAL( sigSendTips( QString ) ), this, SIGNAL( sigSendTips( QString ) ) );

    agvhardware_ = new Agvhardware( argc, argv, this, this );
    agvhardware_->sltAgvHardwareInit();
    navigator_ = new Navigator( argc, argv, this, this );
    navigator_->sltNavigatorInit();

    // agv、navi信号槽联系
    connect( agvhardware_, SIGNAL( sigSendTips( QString ) ), this, SIGNAL( sigSendTips( QString ) ) );
    connect( navigator_, SIGNAL( sigSendTips( QString ) ), this, SIGNAL( sigSendTips( QString ) ) );
    connect( agvhardware_, SIGNAL( sigStartButtonHit() ), navigator_, SLOT( sltStartButtonHit() ) );

    QTimer* time = new QTimer();
    connect( time, SIGNAL( timeout() ), this, SLOT( sltTimeOut() ) );
    time->start( 10 );  //设置为最小周期10ms，函数内部再按SysCommunicationFrequency来计算需要的周期。
}
